#include "WPILib.h"


// This is a demo program showing the use of pneumatics

class PneumaticsRobot : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick stick; // only joystick

public:
	PneumaticsRobot(void):
		myRobot(2,3,4,5),	// these must be initialized in the same order
		stick(1)		// as they are declared above.
	{
		myRobot.SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.MecanumDrive_Cartesian(-0.5, 0.0, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.MecanumDrive_Cartesian(0.0, 0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		/** Initialize the soloenoid with new Solenoid()
			The argument is the channel the solenoid is attached to on the
			solenoid breakout.
		*/
		Solenoid *Solenoid1 = new Solenoid(1);
		
		// Set the solenoid to the non-firing position.
		Solenoid1->Set(false);
		
		/** Initialize the compressor with new Compressor ()
		 * The first argument is the pressure switch relay channel number: connect
		 * this up to one of the cRIO inputs.
		 * The second argument is the Compressor relay channel (The spike will connect):
		 * Connect this up to one of the cRIO outputs.
		 * Initialisze the compressor only ONCE and use it again later. 
		 * Stop the compressor manually with compressor->Stop()
		*/
		Compressor *compressor = new Compressor(9, 8);
		
		//Start the compressor
		compressor->Start();
		
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick);
			
			// If trigger is pressed run the solenoid
			if(stick.GetTrigger()) {
				Solenoid1->Set(true);
			} 
			
			// If trigger is not pressed turn off the solenoid
			else {
				Solenoid1->Set(false);
			}
			Wait(0.005); // wait for a motor update time
		}
	}
	
	/**
	 * Runs during test mode
	 */
	void Test() {

	}
};

START_ROBOT_CLASS(PneumaticsRobot);

